# -*- coding: utf-8 -*-
'''
Created on 27.08.2019

@author: yu03
'''

import matplotlib.pyplot as plt
import numpy as np
import datetime
import os
from scipy import signal
from Video_Experiment import txt_file_set, compare_num, note_set

'''
    读取数据 (4通道)
'''
def Read_Data_4Ch(name):
    '''
        Return Data in File (4 Channels: Data_Ch1, Data_Ch2, Data_Ch3, Data_Ch4)
        File name required (default path)
    '''
    print('Reading Data')
    with open(name,'r') as fid:
        line=''
        while line[0:4] != '----':
            line = fid.readline()
            print(line)
            if line[0:2] == 'Fs':
                p, q, m, n = line.strip().split(' ')
                Fs = float(m)
                print('Fs = %f\n'%Fs)
        out_str = fid.readlines()
    Data_Ch1, Data_Ch2, Data_Ch3, Data_Ch4 = [], [], [], []
    for line in out_str:
        a, b, c, d= line.strip().split(', ')
        Data_Ch1.append(float(a))
        Data_Ch2.append(float(b))
        Data_Ch3.append(float(c))
        Data_Ch4.append(float(d))    
    Data_Ch1 = np.array(Data_Ch1)
    Data_Ch2 = np.array(Data_Ch2)
    Data_Ch3 = np.array(Data_Ch3)
    Data_Ch4 = np.array(Data_Ch4)
    return Data_Ch1, Data_Ch2, Data_Ch3, Data_Ch4, Fs


Lamda = 633e-9
pix_size = 5.3e-6
V_x, V_y, V_z = 0, 0, 0
now = datetime.datetime.now()

hor_length_nonlinearity_set = []
ver_length_nonlinearity_set = []
reference_set = []
sum_set = []
diff_set = []

for i in range(compare_num):
    hor_f_fit_set, ver_f_fit_set, hor_phase_centers, ver_phase_centers, fs_cam = Read_Data_4Ch(txt_file_set[i])
    hor_angle = (V_x-Lamda*hor_f_fit_set/2)*1e6 ### urad
    hor_length = np.unwrap(hor_phase_centers)/4/np.pi*Lamda*1e9 ### nm
    ver_angle = (V_y-Lamda*ver_f_fit_set/2)*1e6 ### urad
    ver_length = np.unwrap(ver_phase_centers)/4/np.pi*Lamda*1e9 ### nm
    
    [p, q] = np.polyfit(np.linspace(0, len(hor_length)-1, len(hor_length)), hor_length, 1)
    linear_fit = np.linspace(0, len(hor_length)-1, len(hor_length))*p + q
    hor_length_nonlinearity = (hor_length - linear_fit)
    reference = linear_fit
    
    [p, q] = np.polyfit(np.linspace(0, len(ver_length)-1, len(ver_length)), ver_length, 1)
    linear_fit = np.linspace(0, len(ver_length)-1, len(ver_length))*p + q
    ver_length_nonlinearity = (ver_length - linear_fit)

    hor_length_nonlinearity_set.append(hor_length_nonlinearity)
    ver_length_nonlinearity_set.append(ver_length_nonlinearity)
    reference_set.append(reference)
    sum_set.append((hor_length+ver_length)/2)
    diff_set.append((hor_length-ver_length)/2)

fig = plt.figure('Nonlinearity')
ax = fig.subplots(2, 4, sharex='col', sharey='row')
for i in range(compare_num):
    axis = ax[i//4, i%4]
    y = hor_length_nonlinearity_set[i]
    x = np.arange(len(y))/fs_cam
    axis.plot(x, y, color='blue', marker=' ', label=note_set[i])
    axis.grid(which='both', axis='both')
    y = ver_length_nonlinearity_set[i]
    x = np.arange(len(y))/fs_cam
    axis.plot(x, y, color='red', marker=' ',)
    legend = axis.legend(loc='upper right')
#     plt.setp(legend.get_texts()[0], color = 'blue')
#     plt.setp(legend.get_texts()[1], color = 'red')

axis = ax[1, 3]
axis.text(0, 5, 'Horizontal', color='blue')
axis.text(0, 4, 'Vertical', color='red')
fig.text(0.5, 0.04, 'Time (s)', ha='center')
fig.text(0.04, 0.5, 'Length (nm)', va='center', rotation='vertical')

# #     plt.subplot(compare_num, 3, 3+3*i)
#     y = hor_length_nonlinearity_set[i] + ver_length_nonlinearity_set[i]
#     x = np.arange(len(y))/fs_cam
#     ax[i, 2].plot(x, y, color='blue', marker=' ')
# #     ax[i, 2].plot(x, hor_length_nonlinearity_set[i]-ver_length_nonlinearity_set[i], color='red', marker=' ')
#     plt.ylabel("Comb. Length (nm)")
#     plt.xlabel("Time (s)")
#     plt.grid(which='both', axis='both')

fig = plt.figure('Combination')
ax = fig.subplots(2, 4, sharex='col', sharey='row')
for i in range(compare_num):
    axis = ax[i//4, i%4]
#     y = (hor_length_nonlinearity_set[i]+ver_length_nonlinearity_set[i])/2
    y = sum_set[i] - reference_set[i]
    x = np.arange(len(y))/fs_cam
#     axis.plot(x, y, color='blue', marker=' ', label=note_set[i])
    axis.grid(which='both', axis='both')
#     y = (hor_length_nonlinearity_set[i]-ver_length_nonlinearity_set[i])/2
    y = diff_set[i] - reference_set[i]
    y = y - np.average(y)
    x = np.arange(len(y))/fs_cam
    axis.plot(x, y, color='red', marker=' ', label=note_set[i])
    legend = axis.legend(loc='upper right')
axis = ax[1, 3]
# axis.text(0, 0, 'Sum', color='blue')
# axis.text(0, 50, 'Diff', color='red')
fig.text(0.5, 0.04, 'Time (s)', ha='center')
fig.text(0.04, 0.5, 'Length (nm)', va='center', rotation='vertical')
plt.show()